# -*-coding:utf-8-*-
__author__ = 'wss'

import heapq
from graph import *
from draw_graph import *
from math import *

class PriorityQueue:
    def __init__(self):
        self.elements = []

    def empty(self):
        return len(self.elements)==0

    def put(self, item, priority):
        heapq.heappush(self.elements, (priority, item))  # 往堆中插入一条新的值

    def get(self):
        return heapq.heappop(self.elements)[1]  # 返回root节点，即heap中最小的元素

def heuristic(a, b):
    (x1, y1) = a
    (x2, y2) = b
    abs_x = abs(x1 - x2)
    abs_y = abs(y1 - y2)
    return min((abs_x,abs_y))*sqrt(2)+max((abs_x,abs_y))-min((abs_x,abs_y))
    # return sqrt(abs(x1 - x2)*abs(x1 - x2) + abs(y1 - y2)*abs(y1 - y2))

def a_star_search(graph, start, goal):
    frontier = PriorityQueue()  # 定义一个空堆
    frontier.put(start, 0)  # 放入起点和路径权重
    came_from = {}
    cost_so_far = {}
    came_from[start] = None
    cost_so_far[start] = 0

    while not frontier.empty():  # 如果堆不为空
        current = frontier.get()  # 从堆中取出最小的元素

        if current == goal:  # 如果到达目标点，即路径最小权重属于目标点，退出
            break

        for next in graph.neighbors(current):  # 对当前最小权重点位的邻居进行判断
            # if next not in cost_so_far:
            new_cost = cost_so_far[current] + graph.cost(current, next)  # 让新的权重等于起点到当前点的权重+当前点到邻居的权重
            if next not in cost_so_far or new_cost < cost_so_far[next]:  # 如果邻居不在原最短路径中，或加入新邻居后总权重小于原权重
                cost_so_far[next] = new_cost  # 往路径中添加该邻居，记录实际权重
                priority = new_cost + heuristic(goal, next)  # 计算启发式权重=新权重+新邻居到目标的预估权重
                frontier.put(next, priority)  # 将新邻居和启发式权重放入堆中
                came_from[next] = current  # 记录新的路径方向

    return came_from, cost_so_far

def main():
    # 定义10*10网格图，同时定义各方向权重
    random.seed(2)
    diagram5 = GridWithWeights_new(10, 10)
    diagram5.walls = [(1, 7), (1, 8), (2, 7), (2, 8), (3, 7), (3, 8)]
    for idx_x in range(10):
        for idx_y in range(10):
            from_node = (idx_x, idx_y)
            if from_node not in diagram5.walls:  # 出发结点不能是障碍结点
                from_node_neighbors = diagram5.neighbors(from_node)  # 找到出发结点的邻居（肯定不是障碍结点或超范围结点）
                if len(from_node_neighbors) > 0:
                    for neighbor in from_node_neighbors:
                        val = random.randrange(1,10)
                        if not diagram5.weights.get((neighbor, from_node)):
                            if val == 9:
                                diagram5.weights[(from_node, neighbor)] = 50
                            else:
                                diagram5.weights[(from_node, neighbor)] = val
                        else:
                            diagram5.weights[(from_node, neighbor)] = diagram5.weights.get((neighbor, from_node))

    start, goal = (1, 4), (7, 8)
    came_from, cost_so_far = a_star_search(diagram5, start, goal)
    draw_grid(diagram5, width=3, point_to=came_from, start=start, goal=goal)
    print ''
    draw_grid(diagram5, width=3, number=cost_so_far, start=start, goal=goal)
    print ''
    draw_grid(diagram5, width=3, path=reconstruct_path(came_from, start=start, goal=goal))
    print 'cost:%d'%cost_so_far.get(goal)

    start, goal = (7, 8), (1, 4)
    came_from, cost_so_far = a_star_search(diagram5, start, goal)
    draw_grid(diagram5, width=3, point_to=came_from, start=start, goal=goal)
    print ''
    draw_grid(diagram5, width=3, number=cost_so_far, start=start, goal=goal)
    print ''
    draw_grid(diagram5, width=3, path=reconstruct_path(came_from, start=start, goal=goal))
    print 'cost:%d' % cost_so_far.get(goal)
    print ''


if __name__ =='__main__':
    main()